/**
 * @file PartialAR.cpp
 * Class to compute the PPP Solution.
 *
 * modification
 *
 * 2020/01/22
 * add lambda and partial-ambiguity fixing method.
 *
 * 2020/01/25
 * type-related bias is computed and stored.
 * 
 * 2020/01/26
 * choose the robust receiver bias datum using median method
 * instead of previous ambiguity-datum
 *
 * 2020/02/06
 * an robust iterative receiver bias selection method is introduced.
 *
 * 2020/02/07 
 * update the interface of all functions 
 *
 * 2020/02/07
 * datum sat ambiguity for real-time ppp
 * 
 * @author shjzhang 2019/07/01
 * 
 */

#include "Edge.hpp"
#include "PartialAR.hpp"
#include "MatrixFunctors.hpp"
#include "Matrix.hpp"
#include "Kruskal.hpp"
#include "Counter.hpp"
#include "DataStructures.hpp"
#include "Stats.hpp"

#define debug 1

namespace gpstk
{

      // Returns a string identifying this object.
    std::string PartialAR::getClassName() const
    { return "PartialAR"; }

    void PartialAR::init()
    {
        // do nothing until now
    };

    void PartialAR::Round(VariableEstDataMap& ambEstData,
                          VariableDataMap& fixedAmbData)
    {
        VariableDataMap ambData, ambVarData;
        for(auto aed: ambEstData)
        {
            ambData[aed.first] = aed.second.value;
            ambVarData[aed.first] = aed.second.variance;
        }

        // now, let's fix the ambiguities
        Round(ambData, ambVarData, fixedAmbData);
    }

    void PartialAR::Round(VariableDataMap& ambData,
                          CovarianceDataMap& ambCovData, 
                          VariableDataMap& fixedAmbData)
    {
        VariableDataMap ambVarData;
        for(auto avd: ambCovData)
        {
            ambVarData[avd.first] = ambCovData[avd.first][avd.first];
        }

        // now, let's fix the ambiguities
        Round(ambData, ambVarData, fixedAmbData);

    }

    void PartialAR::Round(VariableDataMap& ambData,
                          VariableDataMap& ambVarData, 
                          VariableDataMap& fixedAmbData)
    {
        VariableDataMap ambDataFixed;
        for( auto amb : ambData )
        {
            double ambFloat;
            ambFloat = amb.second;

            double ambVar;
            ambVar = ambVarData[amb.first];

              // ambiguity fixing
            ARData arData;
            arData = ambRound.resolve(ambFloat, ambVar);
            if(arData.fixed)
            {
                ambDataFixed[amb.first] = arData.ambFixed;
            }
        }
        fixedAmbData = ambDataFixed;
    }


    double PartialAR::PAR(VariableDataMap& ambData, 
                        CovarianceDataMap& ambCovData,
                        VariableDataMap& fixedAmbData)
    {

        VariableDataMap tempAmbData = ambData;
        CovarianceDataMap tempAmbCovData = ambCovData;
        VariableDataMap tempFixedAmbData;

        // now, par
        double ratio(0.0);
        while(ratio < 3.0)
        {
            // exception
            if(tempAmbData.size() <= 3)
            {
                break;
            }

            // fix with lambda
            ratio = Lambda(tempAmbData, tempAmbCovData, tempFixedAmbData);

            if(ratio >= 3.0)
            {
                break;
            }
            else
            {
                double minPrec(9.9E+10);
                Variable ambMinPrec;
                for(auto amb: tempAmbData)
                {
                    double ambFloat = tempAmbData[amb.first];
                    double ambVar = tempAmbCovData[amb.first][amb.first];
                    ARData arData = ambRound.resolve(ambFloat, ambVar);

                    if( arData.precision < minPrec )
                    {
                        minPrec = arData.precision;
                        ambMinPrec = amb.first;
                    }
                }

                // erase the ambiguity with max-variance
                tempAmbData.erase(ambMinPrec);
            }
        }

        if(ratio>3.0)
        {
            fixedAmbData = tempFixedAmbData;
        }

        return ratio;

    }

    double PartialAR::Lambda(VariableDataMap& ambData, 
                             CovarianceDataMap& ambCovData,
                             VariableDataMap& fixedAmbData)
    {
        // define ambVec/ambCovMatrix
        double namb = ambData.size();
        Vector<double> ambVec(namb, 0.0);
        Matrix<double> covMatrix(namb, namb, 0.0);

        // convert ambData/ambCovData to ambVec/covMatrix 
        int i = 0;
        for(auto amb : ambData)
        {
            ambVec(i) = amb.second;
            int j = 0;
            for(auto amb2 : ambData)
            {
                covMatrix(i,j) = ambCovData[amb.first][amb2.first];
                j++;
            }
            i++;
        }

        // resolve the ambiguties using MLAMBDA method
        ambMLambda.resolve(ambVec, covMatrix);

        Vector<double> fixedAmbVec(namb, 0.0);
        fixedAmbVec = ambMLambda.getFixedAmbVec();

        // now, save fixedAmbVec into allIntAmbDataFixed;
        i = 0;
        VariableDataMap tempFixedAmbData;
        for(auto amb: ambData)
        {
            tempFixedAmbData[amb.first] = fixedAmbVec[i];
            i++;
        }

        double ratio;
        ratio= ambMLambda.getRatio();

        fixedAmbData = tempFixedAmbData;

        return ratio;
    }

}  // End of namespace gpstk
